#include "motorwidget.h"
#include "ui_motorwidget.h"

motorWidget::motorWidget(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::motorWidget)
{
    ui->setupUi(this);

    //校准左电机
    connect(ui->pushButton,&QPushButton::clicked,this,[=](){
        SendCMD(0x2A,0x01,0xff,0,0,0);
    });
    //校准右电机
    connect(ui->pushButton_2,&QPushButton::clicked,this,[=](){
         SendCMD(0x2A,0x02,0xff,0,0,0);
    });
    //校准左右电机
    connect(ui->pushButton_3,&QPushButton::clicked,this,[=](){
         SendCMD(0x2A,0x03,0xff,0,0,0);
    });

    connect(ui->pushButton_4,&QPushButton::clicked,this,[=](){
        uint8_t func2,func3;
        if(QString::compare(ui->comboBox->currentText(),"暂停左右电机") == 0)
            func2 = 0x21;
        else if(QString::compare(ui->comboBox->currentText(),"开环") == 0)
            func2 = 0x11;
        else if(QString::compare(ui->comboBox->currentText(),"电流控制") == 0)
            func2 = 0x12;
        else if(QString::compare(ui->comboBox->currentText(),"速度控制") == 0)
            func2 = 0x16;

        if(QString::compare(ui->comboBox_2->currentText(),"左电机") == 0)
            func3 = 0x01;
        else if(QString::compare(ui->comboBox_2->currentText(),"右电机") == 0)
            func3 = 0x02;
        else if(QString::compare(ui->comboBox_2->currentText(),"左右电机") == 0)
            func3 = 0x03;
        SendCMD(0x2A,func2,func3,0,0,0);


    });

    //校准设置Uq Ud
    connect(ui->pushButton_5,&QPushButton::clicked,this,[=](){
        float Uq = ui->doubleSpinBox->value();
        float Ud = ui->doubleSpinBox_2->value();
        uint8_t func3;
        if(QString::compare(ui->comboBox_5->currentText(),"左电机") == 0)
            func3 = 0x01;
        else
            func3 = 0x02;

        SendCMD(0x2A,0x31,func3,Uq,Ud,0);
    });
    //校准波形max min
    connect(ui->pushButton_6,&QPushButton::clicked,this,[=](){

        char offset;
        uint8_t func3;
        if(QString::compare(ui->comboBox_4->currentText(),"左电机") == 0)
            offset = 0;
        else
            offset = 0x03;

        if(QString::compare(ui->comboBox_3->currentText(),"TaTbTc") == 0)
            func3 = 0x01 + offset;
        else if(QString::compare(ui->comboBox_3->currentText(),"IaIbIc") == 0)
            func3 = 0x02 + offset;
        else
            func3 = 0x03 + offset;

        float max = ui->doubleSpinBox_3->value();
        float min = ui->doubleSpinBox_4->value();

        SendCMD(0x2A,0x41,func3,max,min,0);
    });


    connect(ui->pushButton_7,&QPushButton::clicked,this,&motorWidget::Send_I_Iq);//设置目标电流Iq
    connect(ui->pushButton_9,&QPushButton::clicked,this,&motorWidget::Send_Iq_PID);//设置电流环q的PI参数
    connect(ui->pushButton_11,&QPushButton::clicked,this,&motorWidget::Send_Id_PID);//设置电流环d的PI参数


    connect(ui->pushButton_8,&QPushButton::clicked,this,&motorWidget::Send_Aim_Speed);//设置目标速度
    connect(ui->pushButton_10,&QPushButton::clicked,this,&motorWidget::Send_Speed_PID);//设置速度环的PI参数



    connect(ui->comboBox_7,&QComboBox::currentTextChanged,this,[=](const QString &name){
        if(QString::compare(name,"开环") == 0)
            ui->stackedWidget->setCurrentIndex(0);
        else if(QString::compare(name,"电流环") == 0)
            ui->stackedWidget->setCurrentIndex(1);
        else if(QString::compare(name,"速度环") == 0)
            ui->stackedWidget->setCurrentIndex(2);
        else if(QString::compare(name,"位置") == 0)
            ui->stackedWidget->setCurrentIndex(3);
    });
}

motorWidget::~motorWidget()
{
    delete ui;
}


void motorWidget::readResponse(QByteArray data, QString hex_string){
    ui->label_19->setText(QString(data));
}
//设置目标电流Iq
void motorWidget::Send_I_Iq(){
    uint8_t func3;
    if(QString::compare(ui->comboBox_6->currentText(),"左电机") == 0)
        func3 = 0x01;
    else
        func3 = 0x04;

    float Iq = ui->doubleSpinBox_5->value();

    SendCMD(0x2A,0x13,func3,Iq,0,0);

}

//设置电流环q的PI参数
void motorWidget::Send_Iq_PID(){
    char offset;
    uint8_t func3;
    if(QString::compare(ui->comboBox_6->currentText(),"左电机") == 0)
        offset = 0;
    else
        offset = 0x03;

    func3 = 0x02 + offset;


    float Pq = ui->doubleSpinBox_6->value();
    float Pi = ui->doubleSpinBox_7->value();
    SendCMD(0x2A,0x13,func3,0,Pq,Pi);
}

//设置电流环d的PI参数
void motorWidget::Send_Id_PID(){
    char offset;
    uint8_t func3;
    if(QString::compare(ui->comboBox_6->currentText(),"左电机") == 0)
        offset = 0;
    else
        offset = 0x03;

    func3 = 0x03 + offset;
    float Dq = ui->doubleSpinBox_8->value();
    float Di = ui->doubleSpinBox_9->value();
    SendCMD(0x2A,0x13,func3,0,Dq,Di);
}

//发送目标速度
void motorWidget::Send_Aim_Speed(){
    uint8_t func3;
    if(QString::compare(ui->comboBox_8->currentText(),"左电机") == 0)
        func3 = 0x01;
    else
        func3 = 0x03;

    float speed = ui->doubleSpinBox_10->value();
    SendCMD(0x2A,0x17,func3,speed,0,0);
}
//发送速度PI参数
void motorWidget::Send_Speed_PID(){
    char offset;
    uint8_t func3;
    if(QString::compare(ui->comboBox_6->currentText(),"左电机") == 0)
        offset = 0x02;
    else
        offset = 0x04;

    func3 = offset;

    float speed_q = ui->doubleSpinBox_11->value();
    float speed_i = ui->doubleSpinBox_12->value();

    SendCMD(0x2A,0x17,func3,0,speed_q,speed_i);
}


void motorWidget::SendCMD(uint8_t func1,uint8_t func2,uint8_t func3,float data1,float data2,float data3){
    uint8_t cmd[24];
    for(int i = 0 ;i< 24;i++){
        cmd[i] = 0xff;
    }
    cmd[0] = 0xEF;
    cmd[1] = func1;
    cmd[2] = func2;
    cmd[3] = func3;

    char dd[4];
    memcpy(dd,&data1,sizeof (4));
    cmd[4] = dd[0];
    cmd[5] = dd[1];
    cmd[6] = dd[2];
    cmd[7] = dd[3];

    memcpy(dd,&data2,sizeof (4));
    cmd[8] = dd[0];
    cmd[9] = dd[1];
    cmd[10] = dd[2];
    cmd[11] = dd[3];

    memcpy(dd,&data3,sizeof (4));
    cmd[12] = dd[0];
    cmd[13] = dd[1];
    cmd[14] = dd[2];
    cmd[15] = dd[3];

    cmd[23] = 0xFE;
    emit sendData((char *)cmd,sizeof(cmd));   //发送命令
}
